#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "controlcan.h"
using namespace std::chrono_literals;

// 3.自定义节点类；
class Mynode : public rclcpp::Node
{
public:
  Mynode() : Node("can_demo_node")
  {
    // 初始化CAN设备
    device_type = VCI_USBCAN2; // 设备类型修改，在controlcan.h中定义，为4
    device_index = 0;          // 设备的索引 ，第一个设备
    can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1


    // 打开CAN设备
    if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "无法开启CAN设备");
      return;
    }

    //获取设备信息
    if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "获取设备信息失败");
      VCI_CloseDevice(device_type, device_index);
      return;
    }

    // 设置CAN初始化配置
    init_config.AccCode = 0X80000000; // 过滤验收码
    init_config.AccMask = 0xFFFFFFFF; // 过滤屏蔽码
    init_config.Filter = 2; // 只接受标准帧
    init_config.Timing0 = 0x00;  //波特率T2 设置
    init_config.Timing1 = 0x14; // 波特率：1000kps ，波特率T1 设置
    init_config.Mode = 0; // 正常模式

    if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "------初始化CAN参数失败------");
      VCI_CloseDevice(device_type, device_index);
      return;
    }

    // 开始CAN通讯
    if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "------开启CAN通道失败------");
      VCI_CloseDevice(device_type, device_index);
      return;
    }
    
    // 启动电机
    BYTE Open[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc};
    // 输出1NM
    BYTE torque_1[8] = {0x7f, 0xff, 0x7f, 0xf0, 0x00, 0x00, 0x08, 0x71};
    // 输出2NM
    BYTE torque_2[8] = {0x7f, 0xff, 0x7f, 0xf0, 0x00, 0x00, 0x08, 0xe3};
    // 置零
    BYTE close[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd};
    // 关闭
    BYTE zero[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe};

    RCLCPP_INFO(this->get_logger(), "------电机已启动------");
    SendData(config_node, 0x00000001, Open);
    std::this_thread::sleep_for(100ms);
    RCLCPP_INFO(this->get_logger(), "------数据已发送------");
    SendData(config_node, 0x00000001, torque_2);
    std::this_thread::sleep_for(5s);
    // SendData(config_node, 0x00000001, zero);
    // std::this_thread::sleep_for(100ms);
    SendData(config_node, 0x00000001,close);
    std::this_thread::sleep_for(100ms);
    RCLCPP_INFO(this->get_logger(), "------电机已关闭------");

    VCI_CloseDevice(device_type, device_index);
    RCLCPP_INFO(this->get_logger(), "------关闭CAN设备------");
  }

private:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  // USB-CAN系列接口卡的设备信息
  VCI_BOARD_INFO board_info;
  // 初始化CAN的配置
  VCI_INIT_CONFIG init_config;
  // CAN帧结构体
  VCI_CAN_OBJ config_node;
  DWORD device_type;
  DWORD device_index;
  DWORD can_index;

  void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data)
  {
    // 帧ID
    handle_obj.ID = id;
    // 是否是远程帧。=0时为数据帧，=1时为远程帧（数据段空）。
    handle_obj.RemoteFlag = 0;
    // 是否是扩展帧。=0时为标准帧（11位ID），=1时为扩展帧（29位ID）。
    handle_obj.ExternFlag = 0;
    // 数据长度 DLC (<=8)，即CAN帧Data有几个字节。
    handle_obj.DataLen = 8;
    for (int i = 0; i < 8; i++)
    {
      handle_obj.Data[i] = data[i];
    }
    // VCI_Transmit（发送函数） 设备类型 设备索引 CAN通道索引 要发送的帧结构体 VCI_CAN_OBJ数组的首指针 发送的帧数量
    if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
    {
    }
    else
    {
      RCLCPP_FATAL(this->get_logger(), "------USB-CAN设备不存在或USB掉线------");
    }
  }
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(std::make_shared<Mynode>());
  // 5.资源释放
  rclcpp::shutdown();
  return 0;
}